#include <chrono>
#include <cmath>
#include <iostream>
#include <stdint.h>
#include <stdio.h>
#include <thread>
#include <unitree/robot/go2/sport/sport_client.hpp>

int main(int argc, const char** argv)
{
    if (argc < 5) {
        std::cerr << "Please provide vx, vy, vyaw, and duration (seconds) as command line arguments." << std::endl;
        return -1;
    }

    float vx = std::stof(argv[1]); // 从命令行接收 vx
    float vy = std::stof(argv[2]); // 从命令行接收 vy
    float vyaw = std::stof(argv[3]); // 从命令行接收 vyaw
    int duration = std::stoi(argv[4]); // 从命令行接收持续时间（秒）

    unitree::robot::ChannelFactory::Instance()->Init(0, "eth0");
    std::cout << "Factory Init" << std::endl;

    unitree::robot::go2::SportClient spt;

    spt.SetTimeout(10.0f);
    spt.Init();

    // 记录程序开始时间
    auto start_time = std::chrono::high_resolution_clock::now();

    while (true) {

        // 超过指定时间后自动退出
        auto elapsed_time = std::chrono::high_resolution_clock::now() - start_time;
        if (std::chrono::duration_cast<std::chrono::seconds>(elapsed_time).count() >= duration) {
            break;
        }

        std::cout << "vx: " << vx << std::endl;
        std::cout << "vy: " << vy << std::endl;
        std::cout << "vyaw: " << vyaw << std::endl;
        spt.Move(vx, vy, vyaw);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    spt.StopMove();
    std::cout << "Exiting program after " << duration << " seconds." << std::endl;

    return 0;
}
